#include "OpenGLNode.h"

using namespace std;

OpenGLNode::OpenGLNode(): nh_ ("~"){
  string openGLDebug="/openGLDebug";
  openglSub=nh_.subscribe(openGLDebug, 1, &OpenGLNode::NewControllerMsg,this);
}

OpenGLNode::OpenGLNode(const OpenGLNode& orig){
}

OpenGLNode::~OpenGLNode() {
}

void OpenGLNode::NewControllerMsg(const BonzoSurvivors::Beams& contrMsg){
  beams_=contrMsg;
}

bool OpenGLNode::checkAndCollect(){
  if(ros::ok())
  {
    ros::spinOnce();
    return true;
  }
  return false;
}

BonzoSurvivors::Beams beams_;

OpenGLNode* ogl;

const int TOP=0;
const int BOTTOM=1;

const int WIDTH=1024;
const int HEIGHT=700;

int widthRes=WIDTH;
int heightRes=HEIGHT;

bool mouseDown = false;

float l=0.03f;

float tx=0.0f;
float ty=-0.7;
float tz=-5.0f;

float zoom=0.0f;
float deltaZoom=0.1f;

float viewAngle=45.0f;
float rotationAngle=0.0f;
float delta=5.0f;

float zSfondo=0.8f;

void drawString(void* font, char* text,float x,float y)
{
  glRasterPos2f(x,y);
  for(int i=0;i<strlen(text);i++)
  { 
    glutBitmapCharacter(font,text[i]);
  }
}

float calcDistance(Point point1,Point point2)
{
  return sqrt((point1.x-point2.x)*(point1.x-point2.x)+(point1.y-point2.y)*(point1.y-point2.y)+(point1.z-point2.z)*(point1.z-point2.z));
}

float beamLimit(float limit,float degree)
{
  return limit*(1+std::abs(degree));
}

void drawBallInformation(void* font, Sphere ball)
{
  glPushMatrix();
  glLoadIdentity();
  glTranslatef(0.0f,0.0f,-2.0f);
  glColor3f(0.0f,0.0f,0.0f);
  char* str=new char[200];
  Point orig;
  orig.x=0.0;
  orig.y=0.0;
  orig.z=0.0;
  sprintf(str,"Ball center: x:%f y:%f z:%f Distance from robot = %f Radius = %f\0",ball.center.x,ball.center.y,ball.center.z,calcDistance(orig,ball.center),ball.radius);
  drawString(font,str,-2.9,0.4);
  glPopMatrix();
}

void drawMinInformation(void* font, float min)
{
  glPushMatrix();
  glLoadIdentity();
  glTranslatef(0.0f,0.0f,-2.0f);
  glColor3f(0.0f,0.0f,0.0f);
  char* str=new char[200];
  sprintf(str,"Min=%2.2f\0",min);
  drawString(font,str,-3.5,0.2);
  glPopMatrix();
}

void drawFakeMinInformation(void* font, float fakeMin)
{
  glPushMatrix();
  glLoadIdentity();
  glTranslatef(0.0f,0.0f,-2.0f);
  glColor3f(0.0f,0.0f,0.0f);
  char* str=new char[200];
  sprintf(str,"Fake=%2.2f\0",fakeMin);
  drawString(font,str,-3.5,-0.2);
  glPopMatrix();
}

void switchViewPort(int viewport)
{
  switch(viewport)
  {
    case TOP://top viewport
      glViewport (0,heightRes/3,widthRes,(heightRes/3)*2);
      glMatrixMode (GL_PROJECTION);                       // Select The Projection Matrix
      glLoadIdentity();                          // Reset The Projection Matrix
      gluPerspective(45.0f, 1.0f * widthRes / ((heightRes/3)*2), 1.0f, 100.0f); 
      glMatrixMode (GL_MODELVIEW);			// Select The Modelview Matrix
      glLoadIdentity();								// Reset The Modelview Matrix
      glClear(GL_DEPTH_BUFFER_BIT);
      break;
    case BOTTOM://bottom viewport
      glViewport (0,0,widthRes,heightRes/3);
      glMatrixMode (GL_PROJECTION);                       // Select The Projection Matrix
      glLoadIdentity();                          // Reset The Projection Matrix
      gluPerspective(45.0f, 1.0f * widthRes / (heightRes/3), 1.0f, 100.0f); 
      glMatrixMode (GL_MODELVIEW);			// Select The Modelview Matrix
      glLoadIdentity();								// Reset The Modelview Matrix
      glClear(GL_DEPTH_BUFFER_BIT);
      break;
    default:
      break;
  }
}

void Display()
{
    glClear(GL_COLOR_BUFFER_BIT);
    
    switchViewPort(BOTTOM);
    
    glTranslatef(0.0f,0.0f,-2.0f);
    
    //grey region at bottom
    glColor3f(0.9f,0.9f,0.9f);
    glBegin(GL_QUADS);
    glVertex3f(3.0f,0.5f,zSfondo);
    glVertex3f(-3.0f,0.5f,zSfondo);
    glVertex3f(-3.0f,-0.5f,zSfondo);
    glVertex3f(3.0f,-0.5f,zSfondo);
    glEnd();
    
    //histograms
    float lenght=3.0f;
    float base=-0.35f;
    float height=0.6;
    //base line for histograms
    glColor3f(0.0f,0.0f,0.0f);
    glBegin(GL_LINES);
    glVertex3f(-lenght/2.0f,base,zSfondo+0.1f);
    glVertex3f(lenght/2.0f,base,zSfondo+0.1f);
    glEnd();
    
    float degree1=-80.0*M_PI/180;
    float degree2=80.0*M_PI/180;
    float th=0.35f;
    float penalty=0.2f;
    bool find1=false,find2=false;
    float min=numeric_limits<float>::max();
    int minIndex=0;
    if(!beams_.beams.empty())
    {
      for(BonzoSurvivors::Beams::_beams_type::iterator iter=beams_.beams.begin();iter!=beams_.beams.end();iter++)
      {
        if(iter->degree<=degree1 && iter->value<=th)
          find1=true;
        else if(iter->degree>=degree2 && iter->value<=th)
          find2=true;
        if(min>iter->value)
        {
          min=iter->value;
          minIndex=distance(beams_.beams.begin(),iter);
        }
      }
      
      glPushMatrix();
      glClear(GL_DEPTH_BUFFER_BIT);
      drawMinInformation(GLUT_BITMAP_HELVETICA_18,min);
      if(find1 || find2)
        drawFakeMinInformation(GLUT_BITMAP_HELVETICA_18,min-penalty);
      glPopMatrix();
    }
    
    float limit=2.0f;
    if(!beams_.beams.empty())
    {
      int numValue=beams_.beams.size();
      float widthBars=(float)lenght/(float)numValue;
      int count=beams_.beams.size()-1;
      for(BonzoSurvivors::Beams::_beams_type::iterator iter=beams_.beams.begin();iter!=beams_.beams.end();iter++,count--)
      {
        float barHeight;
        if(iter->value<=beamLimit(limit,iter->degree))
        {
          if((iter->degree<=degree1 || iter->degree>=degree2) && (find1 || find2))
          {
            if((distance(beams_.beams.begin(),iter)<=beams_.beams.size()/2 && minIndex<=beams_.beams.size()/2) || (distance(beams_.beams.begin(),iter)>beams_.beams.size()/2 && minIndex>beams_.beams.size()/2))
              glColor3f(1.0f,0.0f,0.0f);
            else
              glColor3f(1.0f,1.0f,0.0f);
            barHeight=((beamLimit(limit,iter->degree)-iter->value+penalty)*height)/beamLimit(limit,iter->degree);
          }
          else
          {
            glColor3f(0.0f,0.0f,0.0f);
            barHeight=((beamLimit(limit,iter->degree)-iter->value)*height)/beamLimit(limit,iter->degree);
          }
          if(distance(beams_.beams.begin(),iter)==minIndex)
          {
            glColor3f(0.0f,1.0f,0.0f);
            //barHeight=((beamLimit(limit,iter->degree)-iter->value+penalty)*height)/beamLimit(limit,iter->degree);
          }
          glBegin(GL_QUADS);
          glVertex3f(-lenght/2+count*widthBars,base,zSfondo+0.1f);
          glVertex3f(-lenght/2+widthBars+count*widthBars,base,zSfondo+0.1f);
          glVertex3f(-lenght/2+widthBars+count*widthBars,base+barHeight,zSfondo+0.1f);
          glVertex3f(-lenght/2+count*widthBars,base+barHeight,zSfondo+0.1f);
          glEnd();
        } 
        else
        {
          glColor3f(0.0f,0.0f,1.0f);
          barHeight=0.0f;
          glBegin(GL_QUADS);
          glVertex3f(-lenght/2+count*widthBars,base+0.01,zSfondo+0.11f);
          glVertex3f(-lenght/2+widthBars+count*widthBars,base+0.01,zSfondo+0.11f);
          glVertex3f(-lenght/2+widthBars+count*widthBars,base-0.05,zSfondo+0.11f);
          glVertex3f(-lenght/2+count*widthBars,base-0.05,zSfondo+0.11f);
          glEnd();
        }
      }
      //maximum's line
      glColor3f(0.0f,1.0f,0.0f);
      float barHeight=((beamLimit(limit,beams_.beams.at(minIndex).degree)-min)*height)/beamLimit(limit,beams_.beams.at(minIndex).degree);
      glBegin(GL_LINES);
      glVertex3f(-lenght/2,base+barHeight,zSfondo+0.1f);
      glVertex3f(lenght/2,base+barHeight,zSfondo+0.1f);
      glEnd();
    }
    
    switchViewPort(TOP);
    
    glTranslatef(0.0f,-0.8f,-2.8f);
    
    glRotatef(viewAngle,1.0f,0.0f,0.0f);
    glRotatef(rotationAngle,0.0f,1.0f,0.0f);
    
    glTranslatef(0.0f,zoom*sin(viewAngle*M_PI/180),zoom*cos(viewAngle*M_PI/180));
    
    glColor3f(0.0f,1.0f,0.0f);
    glBegin(GL_LINES);
    glVertex3f(20*cos(-180*M_PI/180),0.0f,20*sin(-180*M_PI/180));
    glVertex3f(20*cos(0*M_PI/180),0.0f,20*sin(0*M_PI/180));
    glEnd();
    
    //draw a box corresponding to the robot
    glColor3f(0.0f,0.0f,1.0f);
    glPushMatrix();
    glTranslatef(0.0f,0.0f,0.25f/2.0f);
    glutSolidCube(0.25f);
    glPopMatrix();
    
    //draw a direction line of 
   // glColor3f(0.0f,0.0f,1.0f);
    //glBegin(GL_LINES);
   // glVertex3f(,0.0f,);
   // glVertex3f(,0.0f,);
   // glEnd();
    
    if(!beams_.beams.empty())
    {
      for(BonzoSurvivors::Beams::_beams_type::iterator iter=beams_.beams.begin();iter!=beams_.beams.end();iter++)
      {
        float x=-(iter->value*cos(-90*M_PI/180+iter->degree));
        float y=0.0f;
        float z=(iter->value*sin(-90*M_PI/180+iter->degree));
        //glColor3f(0.05f*iter->value,0.05f*iter->value,0.05f*iter->value);
        glColor3f(0.0f,0.0f,0.0f);
        glBegin(GL_QUADS);
        glVertex3f(x,y,z);
        glVertex3f(x+l,y,z);
        glVertex3f(x+l,y+l*cos(-viewAngle),z+l*sin(-viewAngle));
        glVertex3f(x,y+l*cos(-viewAngle),z+l*sin(-viewAngle));
        glEnd();
        //histogram vision curve
        x=-(beamLimit(limit,iter->degree)*cos(-90*M_PI/180+iter->degree));
        y=0.0f;
        z=(beamLimit(limit,iter->degree)*sin(-90*M_PI/180+iter->degree));
        glColor3f(0.0f,0.0f,1.0f);
        glBegin(GL_QUADS);
        glVertex3f(x,y,z);
        glVertex3f(x+0.01f,y,z);
        glVertex3f(x+0.01f,y+0.01f*cos(-viewAngle),z+0.01f*sin(-viewAngle));
        glVertex3f(x,y+0.01f*cos(-viewAngle),z+0.01f*sin(-viewAngle));
        glEnd();
      }
      if(!boost::math::isnan(beams_.xCenter) && !boost::math::isnan(beams_.yCenter))
      {
        Point center;
        center.x=-beams_.yCenter;
        center.y=0.0f;
        center.z=-beams_.xCenter;
        Sphere ball;
        ball.center=center;
        ball.radius=beams_.zCenter;

        glColor3f(1.0f,0.0f,0.0f);
        glPushMatrix();
        glTranslatef(ball.center.x,ball.center.y,ball.center.z);
        glutSolidSphere(ball.radius,50,50);
        glPopMatrix();
        
        switchViewPort(BOTTOM);
        
        drawBallInformation(GLUT_BITMAP_HELVETICA_18,ball);
      }
    }
    
    glutSwapBuffers();
}

void Resize(int w, int h)
{
  if(h==0) // Prevent A Divide By Zero If The Window Is Too Small
    h=1;
  glViewport(0, 0, w, h);
  widthRes=w;
  heightRes=h;
}

bool Init()
{
  glClearColor(1.0f,1.0f,1.0f, 0.0f);
  glEnable(GL_DEPTH_TEST);
  glEnable (GL_LINE_SMOOTH);
  glEnable (GL_BLEND);
  glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
  glHint (GL_LINE_SMOOTH_HINT, GL_DONT_CARE);
  return true;
}

void NormalKeys(unsigned char key, int x, int y)
{
  switch(key)
  {
    case 27:  exit(0);  //ESC
      break;
    case 'o': zoom-=deltaZoom;
      break;
    case 'p': zoom+=deltaZoom;
      break;
    default:break;
  }
  glutPostRedisplay();
}

void SpecialKeys(int key,int x,int y)
{
  switch(key)
  {
    case GLUT_KEY_UP: viewAngle+=delta;
      break;
    case GLUT_KEY_DOWN: viewAngle-=delta;
			break;
    case GLUT_KEY_LEFT: rotationAngle-=delta;
			break;
    case GLUT_KEY_RIGHT: rotationAngle+=delta;
			break;
  }
  glutPostRedisplay();
}

void Cycle()
{
  if(ogl->checkAndCollect())
    beams_=ogl->getBeams();
  else
    cout<<"ROS status is not OK"<<endl;
  
  glutPostRedisplay();
}

int main (int argc, char** argv)
{
  ros::init (argc, argv, "OpenGL");
  OpenGLNode oglObj;
  ogl=&oglObj;
  
  
  glutInit(&argc, argv);
  
	glutInitWindowPosition(50, 50);
	glutInitWindowSize(WIDTH, HEIGHT);  
	glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE | GLUT_DEPTH);
	glutCreateWindow("Rosgl_from_topic");
	glutReshapeFunc(Resize);
  glutKeyboardFunc(NormalKeys);
  glutSpecialFunc(SpecialKeys);
  glutIdleFunc(Cycle);
  glutDisplayFunc(Display);
  
	if (!Init())
    return 1;
  
  glutMainLoop();
}
